#include <Dynamics/Integrator/symplectic_euler_cpu.h>

namespace PhysLeo
{
    
template<typename T>
void SymplecticEulerCpu<T>::allocateResource(SimData<T>& simData)
{
    Integrator<T>::allocateResource(simData);

    ptr_pos_ = simData.ptr_pos_;
    ptr_vel_ = simData.ptr_vel_;
    ptr_force_ = simData.ptr_force_;
    ptr_mass_ = simData.ptr_mass_;
}

template<typename T>
void SymplecticEulerCpu<T>::advanceTime(float dt)
{
    ptr_force_->setZeroCpu();
    Integrator<T>::internal_force_->internalForce(ptr_pos_, ptr_force_);
    Integrator<T>::velocity_damping_->addDampingForce(ptr_vel_, ptr_force_);
    Integrator<T>::gravity_->addGravityForce(ptr_force_);
    Integrator<T>::fixed_freedom_->clearFixedFreedom(ptr_force_);

    auto pos_ptr = ptr_pos_->ptrCpu();
    auto vel_ptr = ptr_vel_->ptrCpu();
    auto force_ptr = ptr_force_->ptrCpu();
    auto mass_ptr = ptr_mass_->ptrCpu();
    const int vert_num = ptr_pos_->size();

    for (auto vert_id = 0; vert_id < vert_num; ++vert_id)
    {
        vel_ptr[vert_id] += force_ptr[vert_id] / mass_ptr[vert_id] * static_cast<T>(dt);
        pos_ptr[vert_id] += vel_ptr[vert_id] * static_cast<T>(dt);
    }
}

template class SymplecticEulerCpu<float>;
template class SymplecticEulerCpu<double>;

}